#include <chrono>
#include <memory>
#include "jsoncpp/json/json.h"
#include <fstream>
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/string.hpp"
#include <sensor_msgs/msg/laser_scan.hpp>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include "tf2/LinearMath/Quaternion.h"
//#include <geometry_msgs/msg/transform_stamped.hpp>
#include <pcl/common/transforms.h>
#include <Eigen/Geometry>
#include "angle_compare.hpp"
#include "jsonread.hpp"
#include "nav_msgs/msg/path.hpp" 
#include "geometry_msgs/msg/pose_stamped.hpp"
#include <filesystem>
using std::placeholders::_1;
namespace fs = std::filesystem;
//#define auto_process 1
using namespace std::chrono_literals;  // 在类定义前或cpp文件顶部添加 
/* This example creates a subclass of Node and uses std::bind() to register a
 * member function as a callback from the timer. */

class MinimalPublisher : public rclcpp::Node
{
public:
  MinimalPublisher()
  : Node("minimal_publisher"), count_(0)
  {
    reference_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>());
    this->declare_parameter<std::string>("filename0","Nmap0.pcd");
    this->declare_parameter<std::string>("filename1","Nmap1.pcd");
    this->declare_parameter<double>("shiftx",0);
    this->declare_parameter<double>("shifty",0);
    this->declare_parameter<double>("shift_angel",0);
    this->declare_parameter<int>("auto_process",0);
    shift_x= this->get_parameter("shiftx").as_double();
    shift_y= this->get_parameter("shifty").as_double();
    shift_angle =this->get_parameter("shift_angel").as_double();
    auto_process=this->get_parameter("auto_process").as_int();
    filename0=this->get_parameter("filename0").as_string();
    filename1=this->get_parameter("filename1").as_string();
    RCLCPP_INFO(this->get_logger(),"file0=%s file1=%s",filename0.c_str(),filename1.c_str());
    RCLCPP_INFO(this->get_logger(),"shiftx =%7.3f shifty=%7.3f shift_angle=%7.3f",shift_x,shift_y,shift_angle);
    publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("file0_cloud", 10);
    my_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("file1_cloud", 10);
    //pub_cloud = create_publisher<sensor_msgs::msg::PointCloud2>("/output_cloud", 10);
    dsp_cloud.reset(new  pcl::PointCloud<pcl::PointXYZ>);
    target_cloud.reset(new  pcl::PointCloud<pcl::PointXYZ>);
    reference_cloud_.reset(new  pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile(filename0,  *target_cloud);
    pcl::io::loadPCDFile(filename1,  *dsp_cloud);
    // pcl::io::loadPCDFile("/home/orangepi/laser_processor/file0.pcd",  *target_cloud);
    // pcl::io::loadPCDFile("/home/orangepi/laser_processor/file1.pcd",  *dsp_cloud);
    timer_ = this->create_wall_timer(1000ms, std::bind(&MinimalPublisher::timer_callback, this));
    
       // 创建订阅者，QoS深度设为10 
    subshift_ = this->create_subscription<std_msgs::msg::String>("/shift",10,std::bind(&MinimalPublisher::shift_callback, this, _1));
      // 订阅 cloud 消息
      cloud_sub = this->create_subscription<sensor_msgs::msg::PointCloud2>("/cloud", 
        10, std::bind(&MinimalPublisher::cloud_topic_callback, this, std::placeholders::_1));
      global_json("./map/map_name.json");  
       // 初始化Header 
      path_msg.header.stamp  = this->now();
      path_msg.header.frame_id  = "map";  // 必须与TF树匹配[4]()
       // 创建路径发布者 
        pub_path_ = this->create_publisher<nav_msgs::msg::Path>("trajectory", 10);
         // 初始化TF广播器
        tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
      mappose = Map_pose("./map/map_name.json"); 
      mappose.cur_pcd_filename = filename0;
      mappose.read_file_data();
  }

private:
 // 回调函数处理接收到的消息 
 void shift_callback(const std_msgs::msg::String::SharedPtr msg)  {
    RCLCPP_INFO(this->get_logger(), "Received: '%s'", msg->data.c_str());
  // 在此处添加业务逻辑处理代码 
  try{
      Json::Reader reader;
      Json::Value json_val;
      int mode;
      if(reader.parse(msg->data, json_val))
      {
        //double shift_x,shift_y,shift_angle;
        mode = json_val["mode"].asInt();
       if(mode==0)
       {
          shift_x = json_val["x"].asDouble();
          shift_y = json_val["y"].asDouble();
          shift_angle = json_val["angle"].asDouble();
          RCLCPP_INFO(this->get_logger(),"shiftx =%7.3f shifty=%7.3f shift_angle=%7.3f",shift_x,shift_y,shift_angle);
       }  //if(mode==0)
        
        if(mode==2)
        {
          if(reference_cloud_->points.size()>0)
            {
              std::string cloudname;
              cloudname = json_val["filename"].asString();
              pcl::PointCloud<pcl::PointXYZ>::Ptr dp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
              std::size_t sz=reference_cloud_->points.size();
              for(std::size_t i=0;i<sz;++i)
                {
                  if((reference_cloud_->points[i].x<100)&&(reference_cloud_->points[i].x>-100))
                    dp_cloud->points.push_back(reference_cloud_->points[i]);
                }
                dp_cloud->width = static_cast<int>(dp_cloud->points.size());
                dp_cloud->height = 1;
              pcl::io::savePCDFile(cloudname, *dp_cloud);
              RCLCPP_INFO(this->get_logger(),"filename='%s'cloud is ok %d",cloudname.c_str(),dp_cloud->points.size());
            }
          }   //if(mode==2)
        if(mode==3)
          RCLCPP_INFO(this->get_logger(),"file name err!");
          
        
        if(mode==1)
        {
          std::size_t last_dot =filename1.find_last_of('.');
          if (last_dot != std::string::npos) 
          {
            std::string filename_temp;
            filename_temp = filename1.substr(0,  last_dot) + ".ppd";
            std::ofstream out_file;
            out_file.open(filename_temp,  std::ios::trunc);  // 追加模式 
            Json::Value json_value;
            json_value["filename"] = filename1;
            json_value["x"] = shift_x;
            json_value["y"] = shift_y;
            json_value["angle"] = shift_angle;
            Json::StreamWriterBuilder writer_builder;
            std::unique_ptr<Json::StreamWriter> writer(writer_builder.newStreamWriter());
            writer->write(json_value, &out_file);
            out_file.close();
                // 其他常用模式：
                // std::ios::trunc   覆盖写入（默认）
                // std::ios::binary  二进制模式   std::ios::app);  // 追加模式

            RCLCPP_INFO(this->get_logger(),"filename=%s",filename_temp.c_str());
          }
          else {RCLCPP_INFO(this->get_logger(),"file name err!");}
               
        }//if(mode==1)
        
      
      }
     }
     catch(const std::exception& e)
     {
         std::cerr << e.what() << '\n';
     }

}
  void timer_callback()
  {
    if(auto_process==1)  //根据json 执行
    {
    double ctt0,ctt1;
    // auto message = std_msgs::msg::String();
    // message.data = "Hello, world! " + std::to_string(count_++);
    // RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
    sensor_msgs::msg::PointCloud2 output_msg;
    sensor_msgs::msg::PointCloud2 output_msg1;
    pcl::toROSMsg(*target_cloud, output_msg);
    // 关键：复制原始消息的header 
    output_msg.header.frame_id  = "map"; // 同步frame_id和时间戳
    output_msg.header.stamp  = this->now(); // 或保持原时间戳
     publisher_->publish(output_msg);
     ctt0=get_angle(0,-2.0,0.5,target_cloud);
    // RCLCPP_INFO(this->get_logger(),"angle0 =%f point=",ctt0);
    ///////////////////////////////////////////
    dsp_cloud->clear();
    pcl::io::loadPCDFile(filename1,  *dsp_cloud);
      // 创建变换矩阵 
      Eigen::Affine3f transform = Eigen::Affine3f::Identity();
      // 添加平移分量 x,y,z
      transform.translation()  << shift_x, shift_y, 0.0;
      float angle_rad = shift_angle;      // * M_PI / 180.0;  // 转为弧度 
      // 绕Z轴旋转（适用于2D平面变换）
      transform.rotate(Eigen::AngleAxisf(angle_rad,  Eigen::Vector3f::UnitZ()));
      pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>); 
      pcl::transformPointCloud(*dsp_cloud, *transformed_cloud, transform);
     pcl::toROSMsg(*transformed_cloud, output_msg1);
     ctt1=get_angle(0,-2.0,0.5,transformed_cloud);
     RCLCPP_INFO(this->get_logger(),"angle 0 =%f angle1 =%f angle_cha=%7.4f",ctt0,ctt1,ctt0-ctt1);
     // 关键：复制原始消息的header 
     output_msg1.header.frame_id  = "map"; // 同步frame_id和时间戳
     output_msg1.header.stamp  = this->now(); // 或保持原时间戳
     my_publisher_->publish(output_msg1);
    }
    if(auto_process==0)  //计算偏移量
    {
      RCLCPP_INFO(this->get_logger(),"start convert!");
      sensor_msgs::msg::PointCloud2 output_msg;
      sensor_msgs::msg::PointCloud2 output_msg1;
      pcl::toROSMsg(*target_cloud, output_msg);
      Compare_to_shift cts;
      cts.set_source_cloud(target_cloud);   ///map0
      cts.set_target_cloud(dsp_cloud);   //map1
      cts.get_shift();
      // cts.get_shift();
      // cts.get_shift();
      RCLCPP_INFO(this->get_logger(),"end convert!");
    }
    if(auto_process==2) 
    {
      sensor_msgs::msg::PointCloud2 output_msg;
      //pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>); 
      Compare_to_shift cts;
      cts.set_source_cloud(target_cloud);   ///map0
      cts.convert_json_name(filename0);
      cts.read_json_seat();
      int size=cts.get_comp_cloud(target_cloud);
      RCLCPP_INFO(this->get_logger(),"json filenwme= %s",cts.json_name.c_str());
     RCLCPP_INFO(this->get_logger(),"cloud size=%d",size);
     pcl::toROSMsg(*target_cloud, output_msg);
     output_msg.header.frame_id  = "map"; // 同步frame_id和时间戳
     output_msg.header.stamp  = this->now(); // 或保持原时间戳
     publisher_->publish(output_msg);
    }
    if(auto_process==3)  //根据 filename.json主动匹配两点云旋转矩阵
    {
      Compare_to_shift cts;
      cts.set_source_cloud(target_cloud);
      cts.set_target_cloud(dsp_cloud);
      cts.convert_json_name(filename0);  //获取filename0的文件名，生成json扩展名
      
      cts.read_json_seat();
      cts.get_convert_json();
      RCLCPP_INFO(this->get_logger(),"angle0 =%7.4f x= %7.4f y= %7.4f",cts.shift_data.angle,cts.shift_data.x,cts.shift_data.y);
     
    }
    //  if(auto_process==4)  //根据 filename.json主动匹配cloud0 计算坐标
    // {
    //   Compare_to_shift cts;
    //   cts.set_source_cloud(target_cloud);
    //   cts.set_target_cloud(dsp_cloud);
    //   cts.convert_json_name(filename0);  //获取filename0的文件名，生成json扩展名
      
    //   cts.read_json_seat();
    //   cts.get_convert_json();
    //   RCLCPP_INFO(this->get_logger(),"angle0 =%7.4f x= %7.4f y= %7.4f",cts.shift_data.angle,cts.shift_data.x,cts.shift_data.y);

    // }
  }
  void cloud_topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
 {
   // pcl::PointCloud<pcl::PointXYZ> laser_cloud;
   pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    reference_cloud_->clear();
    pcl::fromROSMsg(*msg, *temp_cloud);
    std::size_t size;
    size=temp_cloud->points.size();
    for(std::size_t i=0;i<size;++i)
      if((temp_cloud->points[i].x<100)&&(temp_cloud->points[i].x>-100))
        reference_cloud_->points.push_back(temp_cloud->points[i]);
   // double ctt=get_angle(0,-2.0,0.5,temp_cloud);
   //reference_cloud_
    if(auto_process==4)
    {
      Compare_to_shift cts;
       sensor_msgs::msg::PointCloud2 output_msg;
      pcl::io::loadPCDFile(filename0,  *target_cloud); //重新读入pcd文件
   
      cts.set_source_cloud(target_cloud);
      cts.set_target_cloud(temp_cloud);
      cts.convert_json_name(filename0);  //获取filename0的文件名，生成json扩展名
      cts.read_json_seat();
      cts.get_convert_json();

      pcl::PointCloud<pcl::PointXYZ>::Ptr pub_cloud(new pcl::PointCloud<pcl::PointXYZ>);
      // 创建变换矩阵 
      Eigen::Affine3f tt_transform = Eigen::Affine3f::Identity();
      // 添加平移分量 x,y,z
      tt_transform.translation()<<cts.global_data.x,cts.global_data.y,0.0;
      // 绕Z轴旋转（适用于2D平面变换）
      tt_transform.rotate(Eigen::AngleAxisf(cts.global_data.angle,  Eigen::Vector3f::UnitZ()));
      //.inverse()
      pcl::transformPointCloud(*target_cloud, *pub_cloud, tt_transform);
      RCLCPP_INFO(this->get_logger(),"angle0 =%7.4f x= %7.4f y= %7.4f",cts.shift_data.angle*180/3.1415926,cts.shift_data.x,cts.shift_data.y);
      RCLCPP_INFO(this->get_logger(),"angle0 =%7.4f x= %7.4f y= %7.4f",
              (cts.shift_data.angle+cts.global_data.angle)*180/3.1415926,(cts.shift_data.x+cts.global_data.x),(cts.shift_data.y+cts.global_data.y));
      pcl::toROSMsg(*pub_cloud, output_msg);
      // 关键：复制原始消息的header 
      output_msg.header.frame_id  = "map"; // 同步frame_id和时间戳
      output_msg.header.stamp  = this->now(); // 或保持原时间戳
      publisher_->publish(output_msg);
      
      // 初始化poses数组（存储路径点）
      geometry_msgs::msg::PoseStamped pose_stamped;
      pose_stamped.header.frame_id  = "map";
      pose_stamped.pose.position.x  = cts.shift_data.x+cts.global_data.x;
      pose_stamped.pose.position.y  = cts.shift_data.y+cts.global_data.y;
            // 四元数转换
      tf2::Quaternion q;
      q.setRPY(0,  0, cts.shift_data.angle+cts.global_data.angle);  // 绕Z轴旋转
      pose_stamped.pose.orientation.x  = q.x();
      pose_stamped.pose.orientation.y  = q.y();
      pose_stamped.pose.orientation.z  = q.z();
      pose_stamped.pose.orientation.w  = q.w();
      
      path_msg.poses.push_back(pose_stamped);   // 添加路径点
      pub_path_->publish(path_msg);
      //坐标tf发布
       geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp  = pose_stamped.header.stamp; 
        transform.header.frame_id  = "map";    // 父坐标系 
        transform.child_frame_id  = "base_link"; // 子坐标系
         // 设置变换参数
        transform.transform.translation.x  = pose_stamped.pose.position.x; 
        transform.transform.translation.y  = pose_stamped.pose.position.y; 
        transform.transform.translation.z  = 0.0;
        transform.transform.rotation  = pose_stamped.pose.orientation; 
            // 广播TF变换
        tf_broadcaster_->sendTransform(transform);
        mappose.current_seat_data= cts.shift_data;
        mappose.global_data=cts.global_data;
        mappose.just_map();
       
       // RCLCPP_INFO(this->get_logger(),"change map=%d",mappose.change_flag);
        if(mappose.change_flag==1)
         { 
           mappose.create_pcd_filename();
          RCLCPP_INFO(this->get_logger(),"change map=%s",mappose.cur_pcd_filename.c_str());
          filename0=mappose.cur_pcd_filename;
          mappose.change_flag=0;
         }
        else RCLCPP_INFO(this->get_logger(),"this  map=%s",mappose.cur_pcd_filename.c_str());   
    }
  //  RCLCPP_INFO(this->get_logger(),"angle =%f point=",ctt);
 }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr my_publisher_;

  size_t count_;
  pcl::PointCloud<pcl::PointXYZ>::Ptr reference_cloud_;
  std::string filename0,filename1;
  pcl::PointCloud<pcl::PointXYZ>::Ptr dsp_cloud;//(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud;
  double shift_x,shift_y,shift_angle;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subshift_;
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub;
  int auto_process;
  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_path_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  nav_msgs::msg::Path path_msg;
  Map_pose mappose;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;
}
//手动发布消息
///ros2 topic pub /shift std_msgs/msg/String 'data: "test_message"'
//json
// {
//   "mode": 0, 
//   "x": 2.1,
//   "y": 1.4,
//   "angle": 22.2 
// }
//auto_process=1 时下面有效！！！
//ros2 topic pub /shift std_msgs/msg/String 'data: "{\"mode\":0, \"x\":3.27, \"y\":-0.2508, \"angle\":0.1241}"' --once 
//ros2 topic pub /shift std_msgs/msg/String 'data: "{\"mode\":2, \"filename\":\"cloud0.pcd\"}"' --once 